#include "VirtualObjectRenderer.h"

namespace TUMAR { namespace Snowman {
	VirtualObjectRenderer::VirtualObjectRenderer(void) : nearPlane(1.0), farPlane(100.0)
	{
		this->angle = 0;
		this->scale = 0.75;
		this->scaleFactor = 1.0;
		this->fromFirstToSecond = true;
	}

	VirtualObjectRenderer::~VirtualObjectRenderer(void)
	{
	}

	void VirtualObjectRenderer::OnMarkersFound(const MarkerContainer& markers)
	{
		this->markers.assign(markers.begin(), markers.end());

		Invalidate();
	}

	void VirtualObjectRenderer::TransposeMatrix(const float* input, float* output)
	{
		output[0]  = input[0];
		output[4]  = input[1];
		output[8]  = input[2];
		output[12] = input[3];
		output[1]  = input[4];
		output[5]  = input[5];
		output[9]  = input[6];
		output[13] = input[7];
		output[2]  = input[8];
		output[6]  = input[9];
		output[10] = input[10];
		output[14] = input[11];
		output[3]  = input[12];
		output[7]  = input[13];
		output[11] = input[14];
		output[15] = input[15];
	}

	void VirtualObjectRenderer::Render(void)
	{		
		glEnable( GL_DEPTH_TEST );

		glMatrixMode(GL_PROJECTION);
		glPushMatrix();

		glLoadIdentity();
		gluPerspective(60, 640.0 / 480.0, 0.01, 100.0);

		{
			glMatrixMode(GL_MODELVIEW);

			if(markers.size() == 2)
			{
				Marker marker1 = markers[0].MarkerId == 7236 ? markers[0] : markers[1];
				Marker marker2 = markers[0].MarkerId == 7236 ? markers[1] : markers[0];
				
				if(this->game.IsFinished())
				{
					this->game.Start(
						cv::Vec3d(marker1.Pose[3], marker1.Pose[7], marker1.Pose[11]), 
						cv::Vec3d(marker2.Pose[3], marker2.Pose[7], marker2.Pose[11])
					);

					this->fromFirstToSecond = !this->fromFirstToSecond;
				}

				glPushMatrix();
				{
					this->FaceAt(marker1, marker2);

					if(this->fromFirstToSecond)
					{
						this->DrawBall();
					}

					this->DrawSnowman(0.0);
				}
				glPopMatrix();

				glPushMatrix();
				{
					this->FaceAt(marker2, marker1);

					if(!this->fromFirstToSecond)
					{
						this->DrawBall();
					}

					this->DrawSnowman(1.0);
				}
				glPopMatrix();
			}
		}
		glPopMatrix();
	}

	void VirtualObjectRenderer::FaceAt(const Marker& marker, const Marker& target)
	{
		double radToDecFactor = 180 / 3.14159265;
		double dx = target.Pose[3]  - marker.Pose[3];
		double dy = target.Pose[7]  - marker.Pose[7];
		
		double angleXYplane = atan2(dy, dx) * radToDecFactor;
		
		float* m = new float[16];
		{
			this->TransposeMatrix(marker.Pose, m);			
			glLoadMatrixf(m);
		}
		delete m;

		double beta = atan2(-marker.Pose[8], sqrt(marker.Pose[0]*marker.Pose[0] + marker.Pose[4] * marker.Pose[4]));
		double alpha = atan2(marker.Pose[4] / cos(beta), marker.Pose[0] / cos(beta)) * radToDecFactor;
		
		glRotatef(alpha + 180, 0, 0, 1);
		glRotatef(angleXYplane - 90, 0, 0, -1);
		
		glScalef(10.0, 10.0, 10.0);
		glBegin(GL_LINES);
		glColor3d(0.0, 1.0, 0.0);
		glVertex3d(0, 0, 0);
		glVertex3d(1, 0, 0);
		glColor3d(1.0, 0.0, 0.0);
		glVertex3d(0, 0, 0);
		glVertex3d(0, 1, 0);
		glColor3d(0.0, 0.0, 1.0);
		glVertex3d(0, 0, 0);
		glVertex3d(0, 0, 1);
		glEnd();
		glScalef(0.1, 0.1, 0.1);
	}

	void VirtualObjectRenderer::FaceAt2(const Marker& marker, const Marker& target)
	{
		double radToDecFactor = 180 / 3.14159265;
		glLoadIdentity();

		cv::Vec3d to = normalize(cv::Vec3d(target.Pose[3] -marker.Pose[3], target.Pose[7] - marker.Pose[7], target.Pose[11] - marker.Pose[11]));
		cv::Vec3d to2(marker.Pose[0] * to[0] + marker.Pose[4] * to[1] + marker.Pose[8] * to[1], 
			marker.Pose[1] * to[0] + marker.Pose[5] * to[1] + marker.Pose[9] * to[1],
			marker.Pose[2] * to[0] + marker.Pose[6] * to[1] + marker.Pose[10] * to[1]);

		float* m = new float[16];
		{
			this->TransposeMatrix(marker.Pose, m);
			glMultMatrixf(m);
		}
		delete m;
		
		cv::Vec3d facing = (cv::Vec3d(0, 1, 0));
		
		cv::Vec3d axis = facing.cross(to2);
		double theta = acos(facing.dot(to2)) * radToDecFactor;
		
		glScalef(10.0, 10.0, 10.0);

		glBegin(GL_LINES);
		glColor3f(1, 1, 1);
		glVertex3d(0, 0, 0);
		glVertex3d(to2[0], to2[1], to2[2]);
		glEnd();

		glRotated(theta, axis[0], axis[1], axis[2]);
		
		glBegin(GL_LINES);
		glColor3d(0.0, 1.0, 0.0);
		glVertex3d(0, 0, 0);
		glVertex3d(1, 0, 0);
		glColor3d(1.0, 0.0, 0.0);
		glVertex3d(0, 0, 0);
		glVertex3d(0, 1, 0);
		glColor3d(0.0, 0.0, 1.0);
		glVertex3d(0, 0, 0);
		glVertex3d(0, 0, 1);
		glEnd();
		glScalef(0.1, 0.1, 0.1);
		/*
		glRotatef(-90, 1, 0, 0);
		glScalef(10.0, 10.0, 10.0);
		glBegin(GL_LINES);
		glColor3d(0.0, 1.0, 0.0);
		glVertex3d(0, 0, 0);
		glVertex3d(marker.Pose[0], marker.Pose[4], marker.Pose[8]);
		glColor3d(1.0, 0.0, 0.0);
		glVertex3d(0, 0, 0);
		glVertex3d(marker.Pose[1], marker.Pose[5], marker.Pose[9]);
		glColor3d(0.0, 0.0, 1.0);
		glVertex3d(0, 0, 0);
		glVertex3d(marker.Pose[2], marker.Pose[6], marker.Pose[10]);
		glEnd();
		glScalef(0.1, 0.1, 0.1);
		*/
		/*
		glBegin(GL_LINES);
		glColor3d(0.0, 1.0, 0.0);
		glVertex3d(0, 0, 0);
		glVertex3d(marker.Pose[0], marker.Pose[1], marker.Pose[2]);
		glColor3d(1.0, 0.0, 0.0);
		glVertex3d(0, 0, 0);
		glVertex3d(marker.Pose[4], marker.Pose[5], marker.Pose[6]);
		glColor3d(0.0, 0.0, 1.0);
		glVertex3d(0, 0, 0);
		glVertex3d(marker.Pose[8], marker.Pose[9], marker.Pose[10]);
		glEnd();
		*/
	}

	void VirtualObjectRenderer::DrawBall(void)
	{
		glPushMatrix();
		{
			const cv::Vec3d p = *this->game.GetCurrentBallPosition();

			glTranslated(p[0], p[1], -p[2]);
			glColor3f(1.0, 0.0, 0.0);
			glutSolidSphere(0.4, 10, 10);
		}
		glPopMatrix();
	}

	void VirtualObjectRenderer::DrawSnowman(double red)
	{		
		angle += 0.025;
		//scale += 0.01 * sinf(angle) * 1.25;

		if(angle > 360.0)
			angle = 0.0;

		glRotatef(-90, 1, 0, 0);

		glScalef(2.0 - scale, scale, 2.0 - scale);
		glScalef(1.0, 0.8, 1.0);

		glColor3f(0 + red, 0.9, 0.9);
		glutSolidSphere(2.0, 24, 24);

		glTranslatef(0, 2.5, 0);
		glColor3f(0.95, 0.95, 0.95);
		glutSolidSphere(1.5, 24, 24);

		glTranslatef(0, 2.0, 0);
		glColor3f(1, 1, 1);
		glutSolidSphere(1.0, 24, 24);

		glPushMatrix();
		{
			glTranslatef(0, 0, 0.9);
			glColor3f(1.0, 0, 0);
			glutSolidCone(0.2, 1.0, 24, 24);
		}
		glPopMatrix();

		glPushMatrix();
		{
			glTranslatef(0.35, 0.5, 0.65);
			glColor3f(0, 0, 0);
			glutSolidSphere(0.15, 12, 12);
		}
		glPopMatrix();

		glPushMatrix();
		{
			glTranslatef(-0.35, 0.5, 0.65);
			glColor3f(0, 0, 0);
			glutSolidSphere(0.15, 12, 12);
		}
		glPopMatrix();
	}
}}